
import time, os, sys
import math
from media.sensor import *
from media.display import *
from media.media import *
from machine import UART, FPIOA,PWM
from machine import Pin,Timer
from time import ticks_ms
import _thread
from pid import PID # 需自行上传库
from filter import LowPassFilter # 需自行上传库
from motor import MOTOR # 需自行上传库

fpioa = FPIOA()

fpioa.set_function(44, FPIOA.UART2_TXD)
fpioa.set_function(45, FPIOA.UART2_RXD)
uart= UART(UART.UART2, baudrate=115200, bits=UART.EIGHTBITS, parity=UART.PARITY_NONE, stop=UART.STOPBITS_ONE)

# 定义颜色阈值和blob设置
thresholds = [(0, 90)]        # 二值化阈值
MIN_AREA = 2000               # 最小面积阈值
MAX_AREA = 100000             # 最大面积阈值
MIN_ASPECT_RATIO = 0.3        # 最小宽高比
MAX_ASPECT_RATIO = 3.0        # 最大宽高比

BASE_RADIUS = 45              # 基础半径（虚拟坐标单位）
POINTS_PER_CIRCLE = 24        # 增加采样点使圆形更平滑
PURPLE_THRESHOLD =(73, 88, -16, 45, -6, -24) # 紫色色块阈值 (20, 60, 15, 70, -70, -20)
lcd_width = 800
lcd_height = 480
# 基础矩形比例（可根据实际需求调整）
RECT_WIDTH = 210
RECT_HEIGHT = 95
TARGET_ASPECT_RATIO = RECT_WIDTH / RECT_HEIGHT  # 目标宽高比

# 激光落点与屏幕中心的误差
LASER_X = 5
LASER_Y = 3
# --------------------------- 工具函数 ---------------------------
def calculate_distance(p1, p2):
    return math.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2)

def calculate_center(points):
    if not points:
        return (0, 0)
    sum_x = sum(p[0] for p in points)
    sum_y = sum(p[1] for p in points)
    return (sum_x / len(points), sum_y / len(points))

def is_valid_rect(corners):
    edges = [calculate_distance(corners[i], corners[(i+1)%4]) for i in range(4)]

    ratio1 = edges[0] / max(edges[2], 0.1)
    ratio2 = edges[1] / max(edges[3], 0.1)
    valid_ratio = 0.5 < ratio1 < 1.5 and 0.5 < ratio2 < 1.5

    area = 0
    for i in range(4):
        x1, y1 = corners[i]
        x2, y2 = corners[(i+1) % 4]
        area += (x1 * y2 - x2 * y1)
    area = abs(area) / 2
    valid_area = MIN_AREA < area < MAX_AREA

    width = max(p[0] for p in corners) - min(p[0] for p in corners)
    height = max(p[1] for p in corners) - min(p[1] for p in corners)
    aspect_ratio = width / max(height, 0.1)
    valid_aspect = MIN_ASPECT_RATIO < aspect_ratio < MAX_ASPECT_RATIO

    return valid_ratio and valid_area and valid_aspect

def detect_purple_blobs(img):
    return img.find_blobs(
        [PURPLE_THRESHOLD],
        pixels_threshold=2000,#5000
        area_threshold=2000,#5000
        merge=True
    )

def get_perspective_matrix(src_pts, dst_pts):
    """计算透视变换矩阵"""
    A = []
    B = []
    for i in range(4):
        x, y = src_pts[i]
        u, v = dst_pts[i]
        A.append([x, y, 1, 0, 0, 0, -u*x, -u*y])
        A.append([0, 0, 0, x, y, 1, -v*x, -v*y])
        B.append(u)
        B.append(v)

    # 高斯消元求解矩阵
    n = 8
    for i in range(n):
        max_row = i
        for j in range(i, len(A)):
            if abs(A[j][i]) > abs(A[max_row][i]):
                max_row = j
        A[i], A[max_row] = A[max_row], A[i]
        B[i], B[max_row] = B[max_row], B[i]

        pivot = A[i][i]
        if abs(pivot) < 1e-8:
            return None
        for j in range(i, n):
            A[i][j] /= pivot
        B[i] /= pivot

        for j in range(len(A)):
            if j != i and A[j][i] != 0:
                factor = A[j][i]
                for k in range(i, n):
                    A[j][k] -= factor * A[i][k]
                B[j] -= factor * B[i]

    return [
        [B[0], B[1], B[2]],
        [B[3], B[4], B[5]],
        [B[6], B[7], 1.0]
    ]

def transform_points(points, matrix):
    """应用透视变换将虚拟坐标映射到原始图像坐标"""
    transformed = []
    for (x, y) in points:
        x_hom = x * matrix[0][0] + y * matrix[0][1] + matrix[0][2]
        y_hom = x * matrix[1][0] + y * matrix[1][1] + matrix[1][2]
        w_hom = x * matrix[2][0] + y * matrix[2][1] + matrix[2][2]
        if abs(w_hom) > 1e-8:
            transformed.append((x_hom / w_hom, y_hom / w_hom))
    return transformed

def sort_corners(corners):
    """将矩形角点按左上、右上、右下、左下顺序排序"""
    center = calculate_center(corners)
    sorted_corners = sorted(corners, key=lambda p: math.atan2(p[1]-center[1], p[0]-center[0]))

    # 调整顺序为左上、右上、右下、左下
    if len(sorted_corners) == 4:
        # 找到最接近左上角的点
        left_top = min(sorted_corners, key=lambda p: p[0]+p[1])
        index = sorted_corners.index(left_top)
        sorted_corners = sorted_corners[index:] + sorted_corners[:index]
    return sorted_corners

def get_rectangle_orientation(corners):
    """计算矩形的主方向角（水平边与x轴的夹角）"""
    # 假设排序后的角点顺序为：左上、右上、右下、左下
    if len(corners) != 4:
        return 0

    # 计算上边和右边的向量
    top_edge = (corners[1][0] - corners[0][0], corners[1][1] - corners[0][1])
    right_edge = (corners[2][0] - corners[1][0], corners[2][1] - corners[1][1])

    # 选择较长的边作为主方向
    if calculate_distance(corners[0], corners[1]) > calculate_distance(corners[1], corners[2]):
        main_edge = top_edge
    else:
        main_edge = right_edge

    # 计算主方向角（弧度）
    angle = math.atan2(main_edge[1], main_edge[0])
    return angle

#指示灯
#####################################################################################
LED_G = Pin(20, Pin.OUT, pull=Pin.PULL_NONE, drive=7)  # 绿灯
# 定义闪烁函数
def blink_led(led):
    while True:
        led.value(1)  # 点亮LED
        time.sleep(0.2)  # 等待0.2秒
        led.value(0)  # 熄灭LED
        time.sleep(0.2)  # 等待0.2秒
# 创建并启动线程
_thread.start_new_thread(blink_led, (LED_G,))
#####################################################################################

#舵机
#####################################################################################
# 最小和最大占空比
min_duty = 2.5
max_duty = 13.5
# 中间值，对应于0度
mid_duty = 7.5

## 配置排针引脚号12，芯片引脚号为47的排针复用为PWM通道3输出
pwm_io1 = FPIOA()
pwm_io1.set_function(47, FPIOA.PWM3)
pwm_ud = PWM(3, 50, 50, enable=True)  # 配置PWM3，默认频率50Hz,占空比50%
pwm_ud.duty(7)    #舵机旋转到中间

motor = MOTOR(48, 49, 46, 10000, 0)
motor.set_motor(0)
# 将数值转换为占空比的函数
def input_to_duty_cycle(input_value):

    # 线性映射：y = 0.4x + 7.5
    mapped_output = 0.4 * input_value + 7.5

    # 确保映射后输出在[2.5, 12.5]范围内（防止计算误差）
    if (mapped_output < 2.5):
        mapped_output = 2.5
    elif (mapped_output > 13.5):
        mapped_output = 13.5
    return mapped_output

#####################################################################################

# PID和滤波
#####################################################################################
lr_kp = 0.15 # 0.013
lr_ki = 0.015#0.001
lr_kd = 00.3#0.025
pid_lr = PID(lr_kp, lr_ki, lr_kd,20,50)

ud_kp = 0.005
ud_ki = 0.004
ud_kd = 0.003
pid_ud = PID(ud_kp, ud_ki, ud_kd,6,13.5)

alpha = 0.7  # 滤波器系数，可以根据需要调整这个值
lr_filter = LowPassFilter(0.5)
ud_filter = LowPassFilter(0.9)

#####################################################################################
oneflag = 1

def main():
    try:
        sensor = Sensor(id=2,width=1280, height=960)
        sensor.reset()
        sensor.set_hmirror(True)#False True
        sensor.set_vflip(True)
        sensor.set_framesize(width=320, height=240)
        sensor.set_pixformat(Sensor.RGB565)
        #time.sleep(2)# 延时2秒确保传感器稳定
        # 显示初始化
        Display.init(Display.ST7701, width=lcd_width, height=lcd_height, to_ide=True)
        MediaManager.init()
        sensor.run()
        # 暂时不调用 skip_frames
        # 初始化显示
        clock = time.clock()
        while True:
            clock.tick()
            img = sensor.snapshot()

            # 图像处理
            # 图像转灰度图
            gray_img = img.to_grayscale()
            # 二值化
            binary_img = gray_img.binary(thresholds)
            # 图像膨胀
            binary_img.dilate(2)

            # 检测所有矩形并筛选出最小的一个
            min_area = float('inf')
            smallest_rect = None

            rects = binary_img.find_rects(threshold=5000)
            for rect in rects:
                #给识别到的矩形画十字
                img.draw_cross(rect.x()+(rect.w()//2), rect.y()+(rect.h()//2), color=(255, 255, 255), size=5, thickness=1)
                corners = rect.corners()
                if is_valid_rect(corners):
                    # 计算矩形面积
                    area = 0
                    for i in range(4):
                        x1, y1 = corners[i]
                        x2, y2 = corners[(i+1) % 4]
                        area += (x1 * y2 - x2 * y1)
                    area = abs(area) / 2

                    # 更新最小矩形
                    if area < min_area:
                        min_area = area
                        smallest_rect = corners

            # 只处理最小的矩形
            if smallest_rect:
                # 对矩形角点进行排序
                sorted_corners = sort_corners(smallest_rect)

                # 绘制矩形边框和角点
                for i in range(4):
                    x1, y1 = sorted_corners[i]
                    x2, y2 = sorted_corners[(i+1) % 4]
                    img.draw_line(x1, y1, x2, y2, color=(255, 0, 0), thickness=1)
                for p in sorted_corners:
                    img.draw_circle(p[0], p[1], 5, color=(0, 255, 0), thickness=1)

                cx = sorted_corners[0][0] + ((sorted_corners[1][0]-sorted_corners[0][0])//2)
                cy = sorted_corners[0][1] + ((sorted_corners[3][1]-sorted_corners[0][1])//2)
                img.draw_cross(cx, cy, color=(255, 255, 255), size=5, thickness=1)

                # 计算标靶与屏幕中心的误差
                x_offset = (cx) - img.width() // 2
                y_offset = (cy) - img.height() // 2
                zxc = "y_offset={}, {}".format(x_offset,y_offset)

                # 屏幕显示位置信息和像素大小，包含正负号
                wz ="x={},y={}".format(x_offset,y_offset)
                img.draw_string_advanced(0,30,20,wz)

                # 计算舵机PID
                pid_lr_value = pid_lr.pid_calc(LASER_X,x_offset)
                pid_ud_value = pid_ud.pid_calc(LASER_Y,y_offset)

                #增加电机死区
                pid_lr_value = pid_lr_value + 10
                # 对PID结果滤波
                duty_lr_value = lr_filter.update(-pid_lr_value)
                duty_ud_value = input_to_duty_cycle(pid_ud_value)

                if (duty_ud_value > 7) and (duty_ud_value < 10):
                    pwm_ud.duty(duty_ud_value)
                motor.set_motor(duty_lr_value)

                # 计算矩形主方向角
                angle = get_rectangle_orientation(sorted_corners)

                # 计算矩形实际宽高
                width = calculate_distance(sorted_corners[0], sorted_corners[1])
                height = calculate_distance(sorted_corners[1], sorted_corners[2])
                actual_aspect = width / max(height, 0.1)

                # 确定矩形是横向还是纵向（考虑旋转）
                is_horizontal = actual_aspect >= 1.0

                # 构建虚拟矩形（根据方向调整）
                if is_horizontal:
                    virtual_rect = [
                        (0, 0),
                        (RECT_WIDTH, 0),
                        (RECT_WIDTH, RECT_HEIGHT),
                        (0, RECT_HEIGHT)
                    ]
                else:
                    virtual_rect = [
                        (0, 0),
                        (RECT_HEIGHT, 0),
                        (RECT_HEIGHT, RECT_WIDTH),
                        (0, RECT_WIDTH)
                    ]

                # 计算校正半径（基于实际宽高比）
                if is_horizontal:
                    radius_x = BASE_RADIUS
                    radius_y = BASE_RADIUS / actual_aspect
                else:
                    radius_x = BASE_RADIUS * actual_aspect
                    radius_y = BASE_RADIUS

                # 计算虚拟矩形中心
                virtual_center = (RECT_WIDTH/2, RECT_HEIGHT/2) if is_horizontal else (RECT_HEIGHT/2, RECT_WIDTH/2)

                # 在虚拟矩形中生成椭圆点集（映射后为正圆）
                virtual_circle_points = []
                for i in range(POINTS_PER_CIRCLE):
                    angle = 2 * math.pi * i / POINTS_PER_CIRCLE
                    x = virtual_center[0] + radius_x * math.cos(angle)
                    y = virtual_center[1] + radius_y * math.sin(angle)
                    virtual_circle_points.append((x, y))

                # 计算透视变换矩阵并映射坐标
                matrix = get_perspective_matrix(virtual_rect, sorted_corners)
                if matrix:
                    mapped_points = transform_points(virtual_circle_points, matrix)
                    int_points = [(int(round(x)), int(round(y))) for x, y in mapped_points]

                    # 绘制圆形
                    for (x, y) in int_points:
                        img.draw_circle(x, y, 2, color=(255, 0, 255), thickness=2)

                    # 绘制圆心
                    mapped_center = transform_points([virtual_center], matrix)
                    if mapped_center:
                        cx, cy = map(int, map(round, mapped_center[0]))
                        img.draw_circle(cx, cy, 3, color=(0, 0, 255), thickness=1)
                        center_points=[(int(round(cx)), int(round(cy))) for x, y in mapped_center]

            else:
                motor.set_motor(0)
            # 显示FPS
            fps = clock.fps()
            img.draw_string_advanced(10, 10, 20, f"FPS: {fps:.1f}", color=(255, 255, 255))

            img.draw_cross(img.width() // 2 + LASER_X, (img.height() // 2 + LASER_Y)  , color=(255, 255, 0), size=5, thickness=1)

            # 显示图像
            Display.show_image(img,#gray_img
                              x=round((lcd_width-sensor.width())/2),
                              y=round((lcd_height-sensor.height())/2))

            #print(fps)
            #time.sleep_ms(10)

    except KeyboardInterrupt:
        print("Program stopped by user")
    except Exception as e:
        print(f"Error occurred: {str(e)}")
    finally:
        # 清理资源
        if 'sensor' in locals() and isinstance(sensor, Sensor):
            sensor.stop()
        Display.deinit()
        os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
        time.sleep_ms(100)
        MediaManager.deinit()
        print("System shutdown complete")

if __name__ == "__main__":
    main()
